/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2BroadPhase.h"
#include <algorithm>

#include <cstring>

// Notes: 
// - we use bound array instead of linked list for cache coherence.
// - we use quantized integral values for fast compare.
// - we use short indecies rather than pointers to save memory.
// - we use a stabbing count for fast overlap queries.
// - we also use a time stamp on each proxy to speed up the registration of overlap query result.
// - where possible,we compare bound indices instead of value to reduce cache missing.
// - no broadphase is perfect and neither is this one: it is not greate for huge world. it is not greate for large object.

bool b2BroadPhase::s_validate = false;

struct b2BoundValues
{
	uint16 lowerValues[2];
	uint16 upperValues[2];
};

static int32 BinarySearch(b2Bound* bounds, int32 count, uint16 value)
{
	int32 low = 0;
	int32 high = count - 1;
	while(low <= high)
	{
		int32 mid = (low + high) >> 1;
		if(bounds[mid].value > value)
		{
			high = mid - 1;
		}
		else if(bounds[mid].value < value)
		{
			low = mid + 1;
		}
		else
		{
			return mid;
		}
	}

	return low;
}

b2BroadPhase::b2BroadPhase(const b2AABB& woldAABB, b2PairCallback* callback)
: m_proxyCount(0)
, m_freeProxy(0)
, m_timeStamp(1)
, m_queryResultCount(0)
{
	m_pairManager.Initialize(this, callback);
	b2Assert(woldAABB.IsValid());
	m_worldAABB = woldAABB;
	
	b2Vec2 d = m_worldAABB.upperBound - m_worldAABB.lowerBound;
	m_quantizationFactor.x = float32(B2BROADPHASE_MAX) / d.x;
	m_quantizationFactor.y = float32(B2BROADPHASE_MAX) / d.y;

	for(int32 i=0; i<b2_maxProxies; ++i)
	{
		m_proxyPool[i].SetNext(i+1);
		m_proxyPool[i].timeStamp = 0;
		m_proxyPool[i].overlapCount = b2_invalid;
		m_proxyPool[i].userData = NULL;
	}
	m_proxyPool[b2_maxProxies - 1].SetNext(b2_nullProxy);
}

b2BroadPhase::~b2BroadPhase()
{}

// This one is only used for validation.
bool b2BroadPhase::TestOverlap(b2Proxy* p1, b2Proxy* p2)
{
	for(int32 axis=0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
		b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);

		if(bounds[p1->lowerBounds[axis]].value > bounds[p2->upperBounds[axis]].value)
		{
			return false;
		}

		if(bounds[p2->lowerBounds[axis]].value > bounds[p1->upperBounds[axis]].value)
		{
			return false;
		}
	}

	return true;
}

bool b2BroadPhase::TestOverlap(const b2BoundValues& b, b2Proxy* p)
{
	for(int32 axis = 0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
		b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);

		if(b.lowerValues[axis] > bounds[p->upperBounds[axis]].value)
		{
			return false;
		}

		if(b.upperValues[axis] < bounds[p->lowerBounds[axis]].value)
		{
			return false;
		}
	}

	return true;
}

void b2BroadPhase::ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb)
{
	b2Assert(aabb.lowerBound.x <= aabb.upperBound.x);
	b2Assert(aabb.lowerBound.y <= aabb.upperBound.y);

	b2Vec2 minVetex = b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
	b2Vec2 maxVetex = b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);

	// Bump lower bounds down and upper bound up, this ensure correct of sorting lower/upper bounds witch have the same value.
	lowerValues[0] = (uint16) (m_quantizationFactor.x * (minVetex.x - m_worldAABB.lowerBound.x)) &  (B2BROADPHASE_MAX - 1);
	upperValues[0] = (uint16) (m_quantizationFactor.x * (maxVetex.x - m_worldAABB.lowerBound.x)) | 1;

	lowerValues[0] = (uint16) (m_quantizationFactor.y * (minVetex.y - m_worldAABB.lowerBound.y)) &  (B2BROADPHASE_MAX - 1);
	upperValues[0] = (uint16) (m_quantizationFactor.y * (maxVetex.y - m_worldAABB.lowerBound.y)) | 1;
}

void b2BroadPhase::IncrementTimeStamp()
{
	if(B2BROADPHASE_MAX == m_timeStamp)
	{
		for(int16 i=0; i<b2_maxProxies; ++i)
		{
			m_proxyPool[i].timeStamp = 0;
		}
		m_timeStamp = 1;
	}
	else
	{
		++m_timeStamp;
	}
}

void b2BroadPhase::IncrementOverlapCount(int32 proxyId)
{
	b2Assert((0<=proxyId) && (proxyId<=b2_maxProxies));
	b2Proxy* proxy = m_proxyPool + proxyId;
	if(proxy->timeStamp < m_timeStamp)
	{
		proxy->timeStamp = m_timeStamp;
		proxy->overlapCount = 1;
	}
	else
	{
		proxy->overlapCount = 2;
		b2Assert(m_queryResultCount < b2_maxProxies);
		m_queryResults[m_queryResultCount++] = (uint16)proxyId;
	}
}

void b2BroadPhase::Query(int32* lowerIndex, int32* upperIndex, uint16 lowerValue, uint16 upperValue, b2Bound* bounds, int32 boundCount, int32 axis)
{
	int32 lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
	int32 upperquery = BinarySearch(bounds, boundCount, upperValue);

	// Easy case: lowerQuery <= lowerIndex(i) <= upperQuery
	// Solution: search query range from min bounds.
	for(int32 i=lowerQuery; i<upperquery; ++i)
	{
		if(bounds[i].IsLower())
		{
			IncrementOverlapCount(bounds[i].proxyId);
		}
	}

	// Hard case: lowerIndex(i)< lowerQurey < upperIndex(i)
	// Solution: use the stabbing count to search down the bound array.
	if(lowerQuery > 0)
	{
		int32 i = lowerQuery - 1;
		int32 s = bounds[i].stabbingCount;

		// Find the s stabbing.
		while(s)
		{
			b2Assert(i>=0);
			if(bounds[i].IsLower())
			{
				b2Proxy* proxy = m_proxyPool + bounds[i].proxyId;
				if(lowerQuery <= proxy->upperBounds[axis])
				{
					IncrementOverlapCount(bounds[i].proxyId);
					--s;
				}
			}
			--i;
		}
	}

	*lowerIndex = lowerQuery;
	*upperIndex = upperquery;
}

uint16 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
{
	b2Assert(m_proxyCount < b2_maxProxies);
	b2Assert(b2_nullProxy != m_freeProxy);

	uint16 proxyId = m_freeProxy;
	b2Proxy* proxy = m_proxyPool + proxyId;
	m_freeProxy = proxy->GetNext();

	proxy->overlapCount = 0;
	proxy->userData = userData;

	int32 boundCount =  2 * m_proxyCount;
	uint16 lowerValues[2], upperValues[2];
	ComputeBounds(lowerValues, upperValues, aabb);

	for(int32 axis = 0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		int32 lowerIndex, upperIndex;
		Query(&lowerIndex, &upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);

		memmove(bounds+upperIndex+2, bounds+upperIndex, (boundCount-upperIndex)*sizeof(b2Bound));
		memmove(bounds+lowerIndex+1, bounds+lowerIndex, (upperIndex-lowerIndex)*sizeof(b2Bound));
		
		// The upper index has increased because of the lower bound insertion.
		++upperIndex;

		// Copy in the new bouns.
		bounds[lowerIndex].value = lowerValues[axis];
		bounds[lowerIndex].proxyId = proxyId;
		bounds[upperIndex].value = upperValues[axis];
		bounds[upperIndex].proxyId = proxyId;

		bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex-1].stabbingCount;
		bounds[upperIndex].stabbingCount = bounds[upperIndex - 1].stabbingCount;

		// Adjust the all the affected bounds indices.
		for(int32 index=lowerIndex; index<upperIndex; ++index)
		{
			b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
			if(bounds[index].IsLower())
			{
				proxy->lowerBounds[axis] = (uint16)index;
			}
			else
			{
				proxy->upperBounds[axis] = (uint16)index;
			}
		}
	}

	++m_proxyCount;

	b2Assert(m_queryResultCount < b2_maxProxies);

	// Create pairs if the AABB is in range.
	for(int32 i=0; i<m_queryResultCount; ++i)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());

		m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
	}

	m_pairManager.Commit();

	if(s_validate)
	{
		Validate();
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	return proxyId;
}

void b2BroadPhase::DestroyProxy(int32 proxyId)
{
	b2Assert((0<m_proxyCount) && (m_proxyCount<b2_maxProxies));
	b2Proxy* proxy = m_proxyPool + proxyId;
	b2Assert(proxy->IsValid());

	int32 boundCount = 2 * m_proxyCount;

	for(int32 axis=0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];

		int32 lowerIndex = proxy->lowerBounds[axis];
		int32 upperIndex = proxy->upperBounds[axis];
		uint16 lowerValue = bounds[lowerIndex].value;
		uint16 upperValue = bounds[upperIndex].value;

		memmove(bounds+lowerIndex, bounds+lowerIndex+1, (upperIndex-lowerIndex-1)*sizeof(b2Bound));
		memmove(bounds+upperIndex-1, bounds+upperIndex+1, (boundCount-upperIndex-1)*sizeof(b2Bound));

		//Fix bound indices.
		for(int32 index=lowerIndex; index<boundCount-2; ++index)
		{
			b2Proxy* proxy = m_proxyPool + index;
			if(bounds[index].IsLower())
			{
				proxy->lowerBounds[axis] = (uint16)index;	
			}
			else
			{
				proxy->upperBounds[axis] = (uint16)index;
			}
		}

		//Fix stabbing count.
		for(int32 index = lowerIndex; index<upperIndex-1; ++index)
		{
			--bounds[index].stabbingCount;
		}

		// Qerry for pairs to remove. lowerIndex and upperIndex are not needed.
		Query(&lowerIndex, &upperIndex, lowerValue, upperValue, bounds, boundCount-2, axis);
	}

	b2Assert(m_queryResultCount < b2_maxProxies);

	for(int32 i=0; i<m_queryResultCount;  ++i)
	{
		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
		m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
	}

	m_pairManager.Commit();

	// Prepare for next querry.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	// Return the proxy to the pool.
	proxy->userData = NULL;
	proxy->overlapCount = b2_invalid;
	proxy->lowerBounds[0] = b2_invalid;
	proxy->lowerBounds[1] = b2_invalid;
	proxy->upperBounds[0] = b2_invalid;
	proxy->upperBounds[1] = b2_invalid;
	proxy->SetNext(m_freeProxy);
	m_freeProxy = (uint16)proxyId;
	--m_proxyCount;

	if(s_validate)
	{
		Validate();
	}
}

void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb)
{
	if((b2_nullProxy == proxyId) || (proxyId>b2_maxProxies))
	{
		b2Assert(false);
		return;
	}

	if(aabb.IsValid() == false)
	{
		b2Assert(false);
		return;
	}

	int32 boundCount = 2 * m_proxyCount;
	b2Proxy* proxy = m_proxyPool + proxyId;
	
	// Get new bound values.
	b2BoundValues newValues;
	ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);

	// Get old bound values.
	b2BoundValues oldValues;
	for(int32 axis = 0; axis<2; ++axis)
	{
		oldValues.lowerValues[axis] = m_bounds[axis][proxy->lowerBounds[axis]].value;
		oldValues.upperValues[axis] = m_bounds[axis][proxy->upperBounds[axis]].value;
	}

	for(int32 axis = 0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		int32 lowerIndex = proxy->lowerBounds[axis];
		int32 upperIndex = proxy->upperBounds[axis];

		uint16 lowerValue = newValues.lowerValues[axis];
		uint16 upperValue = newValues.upperValues[axis];

		int32 deltaLower = newValues.lowerValues[axis] - oldValues.lowerValues[axis];
		int32 deltaUpper = newValues.upperValues[axis] - oldValues.upperValues[axis];

		bounds[lowerIndex].value = lowerValue;
		bounds[upperIndex].value = upperValue;

		// Should we move the lower bound down?
		if(deltaLower < 0)
		{
			int32 index = lowerIndex;
			while((index>0) && (lowerValue<bounds[index-1].value))
			{
				b2Bound*	bound = m_bounds[axis] + index;

				b2Bound*	prevBound = bound - 1;
				int32		prevProxyId = prevBound->proxyId;
				b2Proxy*	prevProxy = m_proxyPool + prevProxyId;
				++prevBound->stabbingCount;

				if(prevBound->IsUpper() == true)
				{
					if(TestOverlap(newValues, prevProxy))
					{
						m_pairManager.AddBufferedPair(proxyId, prevProxyId);
					}
	
					++prevProxy->upperBounds[axis];
					++bound->stabbingCount;
				}
				else
				{
					++prevProxy->lowerBounds[axis];
					--bound->stabbingCount;
				}

				--proxy->lowerBounds[axis];
				b2Swap(*bound, *prevBound);
				--index;
			}
		}

		// Should we move the upper bound up?
		if(deltaUpper > 0)
		{
			int32 index = upperIndex;
			while((index<boundCount-1) && (bounds[index+1].value<upperValue))
			{
				b2Bound* bound = bounds + index;
				
				b2Bound* nextBound = bound + 1;
				int32 nextProxyId = nextBound->proxyId;
				b2Proxy* nextProxy = m_proxyPool + nextProxyId;
				++nextBound->stabbingCount;

				if(nextBound->IsLower())
				{
					if(TestOverlap(newValues, nextProxy))
					{
						m_pairManager.AddBufferedPair(proxyId, nextProxyId);
					}

					--nextProxy->lowerBounds[axis];
					++bound->stabbingCount;
				}
				else
				{
					--nextProxy->upperBounds[axis];
					--bound->stabbingCount;
				}

				++proxy->upperBounds[axis];
				b2Swap(*bound, *nextBound);
				++index;
			}
		}

		// Should we move the lower bound up?
		if(deltaLower > 0)
		{
			int32 index = lowerIndex;
			while((index<boundCount-1) && (bounds[index+1].value <= lowerValue))
			{
				b2Bound* bound = bounds + index;
				
				b2Bound* nextBound = bound + 1;
				int32 nextProxyId = nextBound->proxyId;
				b2Proxy* nextProxy = m_proxyPool + nextProxyId;
				--nextBound->stabbingCount;

				if(nextBound->IsUpper())
				{
					if(TestOverlap(oldValues, nextProxy))
					{
						m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
					}

					--nextProxy->upperBounds[axis];
					--bound->stabbingCount;
				}
				else
				{
					--nextProxy->lowerBounds[axis];
					++bound->stabbingCount;
				}

				++proxy->lowerBounds[axis];
				b2Swap(*bound, *nextBound);
				++index;
			}
		}

		// Should we move the upper bound down?
		if(deltaUpper < 0)
		{
			int32 index = upperIndex;
			while((index>0) && (upperValue<bounds[index-1].value))
			{
				b2Bound* bound = bounds + index;
				b2Bound* preBound = bound - 1;
				int32 preProxyId= preBound->proxyId;
				b2Proxy* preProxy = m_proxyPool + preProxyId;
				--preBound->stabbingCount;

				if(preBound->IsLower())
				{
					if(TestOverlap(oldValues, preProxy))
					{
						m_pairManager.RemoveBufferedPair(proxyId, preProxyId);
					}
					
					++preProxy->lowerBounds[axis];
					--bound->stabbingCount;
				}
				else
				{
					++preProxy->upperBounds[axis];
					++bound->stabbingCount;
				}

				--proxy->upperBounds[axis];
				b2Swap(*bound, *preBound);
				--index;
			}
		}
	}

	if(s_validate)
	{
		Validate();
	}
}

void b2BroadPhase::Commit()
{
	m_pairManager.Commit();
}

int32 b2BroadPhase::QuerySegment(const b2Segment& segment, void** userData, int32 maxCount, SortKeyFunc sortKey)
{
	float32 maxLambda = 1.0f;
	float32 dx = (segment.p2.x - segment.p1.x) * m_quantizationFactor.x;
	float32 dy = (segment.p2.y - segment.p1.y) * m_quantizationFactor.y;

	int32 sx = dx < -B2_FLT_EPSILON ? -1 : (dx > B2_FLT_EPSILON ? 1 : 0);
	int32 sy = dy < -B2_FLT_EPSILON ? -1 : (dy > B2_FLT_EPSILON ? 1 : 0);

	b2Assert((0 != sx) || (0 != sy));

	float32 p1x = (segment.p1.x - m_worldAABB.lowerBound.x) * m_quantizationFactor.x;
	float32 p1y = (segment.p1.y - m_worldAABB.lowerBound.y) * m_quantizationFactor.y;

	uint16 startValues[2], startValues2[2];
	int32 xIndex, yIndex;
	
	startValues[0] = (uint16)(p1x) & (B2BROADPHASE_MAX - 1);
	startValues2[0] = (uint16)(p1x) & 1;

	startValues[1] = (uint16)(p1y) & (B2BROADPHASE_MAX - 1);
	startValues2[1] = (uint16)(p1y) & 1;

	// First deal with all the proxies that contains segment.p1
	int32 lowerIndex, upperIndex;
	Query(&lowerIndex, &upperIndex, startValues[0], startValues2[0], m_bounds[0], 2*m_proxyCount, 0);
	if(sx >= 0) xIndex = upperIndex - 1;
	else xIndex = lowerIndex;

	Query(&lowerIndex, &upperIndex, startValues[1], startValues2[1], m_bounds[1], 2 * m_proxyCount, 1);
	if(sy >= 0) yIndex = upperIndex - 1;
	else yIndex = lowerIndex;

	// If we are using sortKey, then sort what we have so far,filtering negative keys.
	if(sortKey)
	{
		// Fill keys.
		for(int32 i=0; i<m_queryResultCount; ++i)
		{
			m_querySortKeys[i] = sortKey(m_proxyPool[m_queryResults[i]].userData);
		}

		// Bubble sort keys
		// Sorting negative values to the top, so we can easily remove them.
		int32 i = 0;
		while(i < m_queryResultCount - 1)
		{
			float32 a = m_querySortKeys[i];
			float32 b = m_querySortKeys[i + 1];
			if((a<0) ? (b>=0) : (a>b && b>0))
			{
				b2Swap(m_querySortKeys[i], m_querySortKeys[i+1]);	// XY change code here.
				b2Swap(m_queryResults[i], m_queryResults[i+1]);
				--i;

				if(-1 == i)		i = 1;
			}
			else
			{
				++i;
			}
		}

		//Skim off negative values.
		while((m_queryResultCount > 0) && (m_querySortKeys[m_queryResultCount-1]<0))
		{
			--m_queryResultCount;
		}
	}

	// Now walk throug the rest segment.
	for(;;)
	{
		float32 xProgress = 0.0f, yProgress = 0.0f;
		//Move on to the next bouond.
		xIndex += sx>=0 ? 1 : -1;
		if((xIndex<0) || (xIndex>=2*m_proxyCount))
		{
			break;
		}
		if(0 != sx)
		{
			xProgress = ((float32)m_bounds[0][xIndex].value - p1x) / dx;
		}

		yIndex += sy>=0 ? 1 : -1;
		if((yIndex<0) || (yIndex >= 2*m_proxyCount))
		{
			break;
		}
		if(0 != sy)
		{
			yProgress = ((float32)m_bounds[1][yIndex].value - p1y) / dy;
		}

		for(;;)
		{
			if((0==sy) || ((0!=sx && xProgress<yProgress)))
			{
				if(xProgress > maxLambda)
				{
					break;
				}

				// Check that we are entering a proxy not leaving.
				if((sx>0) ? m_bounds[0][xIndex].IsLower() : m_bounds[0][xIndex].IsUpper())
				{
					int32 proxyId = m_bounds[0][xIndex].proxyId;
					b2Proxy* proxy = m_proxyPool+proxyId;
					// Check the other axis of the proxy.
					if(sy >= 0)
					{
						if((proxy->lowerBounds[1]<=yIndex-1) && (proxy->upperBounds[1]>=yIndex))
						{
							//Add the proxy.
							if(sortKey)
							{
								AddProxyResult(proxyId, proxy, maxCount, sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount++] = proxyId;
							}
						}
					}
					else
					{
						if((proxy->lowerBounds[1]<=yIndex) && (proxy->upperBounds[1]>=yIndex+1))
						{
							//Add the proxy.
							if(sortKey)
							{
								AddProxyResult(proxyId, proxy, maxCount, sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount++] = proxyId;
							}
						}
					}
				}

				//Early out.
				if(sortKey && m_queryResultCount == maxCount && m_queryResultCount>0 && xProgress>m_querySortKeys[m_queryResultCount-1])
				{
					break;
				}

				//Move on to the next bound.
				if(sx > 0)
				{
					++xIndex;
					if(2*m_proxyCount == xIndex)
					{
						break;
					}
				}
				else
				{
					--xIndex;
					if(xIndex < 0)
					{
						break;
					}
				}
				
				xProgress = ((float32)m_bounds[0][xIndex].value - p1x) / dx;
			}
			else
			{
				if(yProgress > maxLambda)
				{
					break;
				}

				// Check that we are entering a proxy not leaving.
				if((sy>0) ? m_bounds[1][yIndex].IsLower() : m_bounds[1][yIndex].IsUpper())
				{
					int32 proxyId = m_bounds[1][yIndex].proxyId;
					b2Proxy* proxy = m_proxyPool+proxyId;
					// Check the other axis of the proxy.
					if(sx >= 0)
					{
						if((proxy->lowerBounds[0]<=yIndex-1) && (proxy->upperBounds[0]>=yIndex))
						{
							//Add the proxy.
							if(sortKey)
							{
								AddProxyResult(proxyId, proxy, maxCount, sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount++] = proxyId;
							}
						}
					}
					else
					{
						if((proxy->lowerBounds[0]<=yIndex) && (proxy->upperBounds[0]>=yIndex+1))
						{
							//Add the proxy.
							if(sortKey)
							{
								AddProxyResult(proxyId, proxy, maxCount, sortKey);
							}
							else
							{
								m_queryResults[m_queryResultCount++] = proxyId;
							}
						}
					}
				}

				//Early out.
				if(sortKey && m_queryResultCount == maxCount && m_queryResultCount>0 && yProgress>m_querySortKeys[m_queryResultCount-1])
				{
					break;
				}

				//Move on to the next bound.
				if(sy > 0)
				{
					++yIndex;
					if(2*m_proxyCount == yIndex)
					{
						break;
					}
				}
				else
				{
					--yIndex;
					if(yIndex < 0)
					{
						break;
					}
				}

				yProgress = ((float32)m_bounds[1][yIndex].value - p1y) / dy;
			}
		}

		break;
	}

	int32 count = 0;
	for(int32 i=0;i < m_queryResultCount && count<maxCount; ++i, ++count)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Proxy* proxy = m_proxyPool + m_queryResults[i];
		b2Assert(proxy->IsValid());
		userData[i] = proxy->userData;
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	return count;
}

int32 b2BroadPhase::Query(const b2AABB& aabb, void** userData, int32 maxCount)
{
	uint16 lowerValue[2], upperValue[2];
	ComputeBounds(lowerValue, upperValue, aabb);

	int32 lowerIndes, upperIndex;
	Query(&lowerIndes, &upperIndex, lowerValue[0], upperValue[0], m_bounds[0], m_proxyCount * 2, 0);
	Query(&lowerIndes, &upperIndex, lowerValue[1], upperValue[1], m_bounds[1], m_proxyCount * 2, 1);

	b2Assert(m_queryResultCount < b2_maxProxies);

	int32 count = 0;
	for(int32 i=0; i<m_queryResultCount && count<maxCount; ++i, ++count)
	{
		b2Assert(m_queryResults[i] < b2_maxProxies);
		b2Proxy* proxy = m_proxyPool + m_queryResults[i];
		b2Assert(proxy->IsValid());
		userData[i] = proxy->userData;
	}

	// Prepare for next query.
	m_queryResultCount = 0;
	IncrementTimeStamp();

	return count;
}

void b2BroadPhase::Validate()
{
	for(int32 axis=0; axis<2; ++axis)
	{
		b2Bound* bounds = m_bounds[axis];
		int32 boundCount = 2 * m_proxyCount;
		uint32 stabbingCount = 0;

		for(int32 i=0; i<boundCount; ++i)
		{
			b2Bound* bound = bounds + i;
			b2Assert((0==i) || (bounds[i-1].value<=bound->value));
			b2Assert(bound->proxyId != b2_nullProxy);
			b2Assert(m_proxyPool[bound->proxyId].IsValid());

			if(bound->IsLower() == true)
			{
				b2Assert(m_proxyPool[bound->proxyId].lowerBounds[axis] == i);
				++stabbingCount;
			}
			else
			{
				b2Assert(m_proxyPool[bound->proxyId].upperBounds[axis] == i);
				--stabbingCount;
			}

			b2Assert(bound->stabbingCount == stabbingCount);
		}
	}
}

void b2BroadPhase::AddProxyResult(uint16 proxyId, b2Proxy* proxy, int32 maxCount, SortKeyFunc sortKey)
{
	float32 key = sortKey(proxy->userData);
	// Filter the proxies on positive keys.
	if(key < 0)
		return;

	// Merge the new ,key into the sorted list.
	float32* p = m_querySortKeys;
	while((*p<key) && (p<m_querySortKeys+m_queryResultCount))
	{
		++p;
	}

	int32 i = (int32)(p-m_querySortKeys);
	if(maxCount == m_queryResultCount)
	{
		--m_queryResultCount;
	}

	//std::copybackward
	for(int32 j=m_queryResultCount+1; j>i; --j)
	{
		m_querySortKeys[j] = m_querySortKeys[j-1];
		m_queryResults[j] = m_queryResults[j-1];
	}

	m_querySortKeys[i] = key;
	m_queryResults[i] = proxyId;
	++m_queryResultCount;
}
